/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : ekf_uwb_ins.cpp
  * Description        : 
  ******************************************************************************
  * Function           :
  *
  ******************************************************************************
  * Author             : Zhiping Fu
  * Creation Date      : Nov 25, 2020
  ******************************************************************************
  */
/* USER CODE END Header */

#include "ekf_uwb_ins.hpp"

void EKFUWB::EkfUwb_Init()
{
	Update = false;
	dt = 0.005f;
	osDelay(250);

	xQueuePeek(queueUWB, &uwbPos, 0);					//从队列中获取UWB数据

	//参数初始化
	F <<    1.0f,0.0f,  dt,0.0f,0.0f,0.0f,
			0.0f,1.0f,0.0f,  dt,0.0f,0.0f,
			0.0f,0.0f,1.0f,0.0f, -dt,0.0f,
			0.0f,0.0f,0.0f,1.0f,0.0f, -dt,
			0.0f,0.0f,0.0f,0.0f,1.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,0.0f,1.0f;

	B <<    0.0f,0.0f,
			0.0f,0.0f,
			dt  ,0.0f,
			0.0f,dt  ,
			0.0f,0.0f,
			0.0f,0.0f;

	G <<    0.0f,0.0f,0.0f,0.0f,
			0.0f,0.0f,0.0f,0.0f,
			dt  ,0.0f,0.0f,0.0f,
			0.0f,dt  ,0.0f,0.0f,
			0.0f,0.0f,dt  ,0.0f,
			0.0f,0.0f,0.0f,dt  ;

	H <<    1.0f,0.0f,0.0f,0.0f,0.0f,0.0f,
			0.0f,1.0f,0.0f,0.0f,0.0f,0.0f;

	Q <<    1.0f,0.0f,0.0f,0.0f,
			0.0f,1.0f,0.0f,0.0f,
			0.0f,0.0f,0.001f,0.0f,
			0.0f,0.0f,0.0f,0.001f;

	R <<    0.16f,0.0f,
			0.0f,0.16f;

	//初始状态，协方差矩阵

	X_p << uwbPos.uwbPos[0],uwbPos.uwbPos[1],0.0f,0.0f,0.0f,0.0f;

	P_p = P_p = 1e-4f*MatrixXf::Identity(6, 6);


}

void EKFUWB::EkfUwb_Update()
{
	startTimerLast = startTimer;
	getTimer_us(&startTimer);
	cycleTime_us = startTimer - startTimerLast;

	xQueuePeek(queueCb2n, &MatCb2n, 0);					//从队列中获取UWB数据
	xQueuePeek(queueAccDat, &acc, 0);					//从队列中获取加速度计数据
	xQueuePeek(queueUWB, &uwbPos, 0);					//从队列中获取UWB数据

	a_k << acc.acc[0],acc.acc[1],acc.acc[2];			//机体坐标系下的加速度测量值
	Cb2n << MatCb2n.Cb2n[0][0],MatCb2n.Cb2n[0][1],MatCb2n.Cb2n[0][2],
			MatCb2n.Cb2n[1][0],MatCb2n.Cb2n[1][1],MatCb2n.Cb2n[1][2],
			MatCb2n.Cb2n[2][0],MatCb2n.Cb2n[2][1],MatCb2n.Cb2n[2][2];

	accNED = Cb2n*a_k;

	u_k << accNED[0],accNED[1];

	y << uwbPos.uwbPos[0],uwbPos.uwbPos[1];				//NED坐标系下UWB测量值（XY轴位置，单位：m）

	//KF迭代过程

	//Computer Kalman Gain
	K = P_p*H.transpose()*(H*P_p*H.transpose() + R).inverse();

	//Update state
	X_k = X_p + K*(y - H*X_p);

	//Update covariance matrix
	P_k = (MatrixXf::Identity(6, 6) - K*H)*P_p;

	//State prediction
	X_p = F*X_k + B*u_k;

	//Covariance matrix prediction
	P_p = F*P_k*F.transpose() + G*Q*G.transpose();

	ekf_uwb_pos_vel.Pos[0] = X_k[0];
	ekf_uwb_pos_vel.Pos[1] = X_k[1];

	ekf_uwb_pos_vel.Vel[0] = X_k[2];
	ekf_uwb_pos_vel.Vel[1] = X_k[3];

	xQueueOverwrite(queueUwbPosVel,&ekf_uwb_pos_vel);

	getTimer_us(&stopTimer);

	executionTime_us = stopTimer - startTimer;

}

EKFUWB ekfUwb((char *)"EKF_UWB_INS");

extern "C" void ekfUwb_main(void *argument)
{
	ekfUwb.EkfUwb_Init();
	osDelay(5);
	for(;;)
	{
		osSemaphoreAcquire(semEkfUwb,0xffffffff);
		ekfUwb.EkfUwb_Update();
	}
}


/************************ (C) COPYRIGHT Longmen Drone Team *****END OF FILE****/
